Bidirectional Fast Marching Trees: An Optimal Sampling-Based Algorithm for Bidirectional Motion Planning

In this paper, we present the Bi-directional FMT∗(BFMT∗) algorithm for asymptotically-optimal, sampling-based path planning in cluttered, high-dimensional spaces. Specifically, BFMT∗ performs a twosource, lazy dynamic programming recursion over a set of randomlydrawn samples, correspondingly generating two search trees: one in costto-come space from the initial state and another in cost-to-go space from the goal state. This work extends the Fast Marching Tree (FMT∗) algorithm to bi-directional search while preserving its key properties, including probabilistic completeness, hard run-time bounds, and asymptotic optimality through convergence in probability. Multiple strategies are discussed and analyzed for alternating tree growth and interconnecting tree paths. Numerical experiments illustrate the advantages of BFMT∗ over its unidirectional counterpart, FMT∗, as well as a number of other state-of-the-art unidirectional and bidirectional planners.